#ifndef _MOTOR_MPU6050_H_
#define _MOTOR_MPU6050_H_

#include "main.h"
#include "i2c.h"
#include "pid.h"
#include "math.h"
#include "UART\uart_app.h"

//-----------------------------------------------	

#define MPU6050_ADDR        0xD0       // MPU6050 默认地址
#define MPU6050_PWR_MGMT_1  0x6B       // 电源管理寄存器
#define MPU6050_ACCEL_XOUT_H 0x3B      // 加速度计 X 轴高字节寄存器
#define MPU6050_GYRO_XOUT_H  0x43      // 陀螺仪 X 轴高字节寄存器

#define     MPU6050_Add_AD0_0_Write		0xD0
#define     MPU6050_Add_AD0_0_Read		0xD1
#define     MPU6050_Add_AD0_1_Write		0xD2
#define     MPU6050_Add_AD0_1_Read		0xD3

#define		MPU6050_SMPLRT_DIV			0x19
#define		MPU6050_CONFIG				0x1A
#define		MPU6050_GYRO_CONFIG			0x18
#define		MPU6050_ACCEL_CONFIG		0x1C

#define		MPU6050_ACCEL_XOUT_H		0x3B
#define		MPU6050_ACCEL_XOUT_L		0x3C
#define		MPU6050_ACCEL_YOUT_H		0x3D
#define		MPU6050_ACCEL_YOUT_L		0x3E
#define		MPU6050_ACCEL_ZOUT_H		0x3F
#define		MPU6050_ACCEL_ZOUT_L		0x40
#define		MPU6050_TEMP_OUT_H			0x41
#define		MPU6050_TEMP_OUT_L			0x42
#define		MPU6050_GYRO_XOUT_H			0x43
#define		MPU6050_GYRO_XOUT_L			0x44
#define		MPU6050_GYRO_YOUT_H			0x45
#define		MPU6050_GYRO_YOUT_L			0x46
#define		MPU6050_GYRO_ZOUT_H			0x47
#define		MPU6050_GYRO_ZOUT_L			0x48
                                         
#define		MPU6050_PWR_MGMT_1			0x6B
#define		MPU6050_PWR_MGMT_2			0x6C
#define		MPU6050_WHO_AM_I			0x75

#define MPU6050_ADDR 0xD0//从机地址
#define SMPLRT_DIV_REG 0x19//采样率
#define MPU6050_CONFIG 0x1A
#define GYRO_CONFIG_REG 0x1B//陀螺仪
#define ACCEL_CONFIG_REG 0x1C//加速度
#define ACCEL_XOUT_H_REG 0x3B
#define TEMP_OUT_H_REG 0x41
#define GYRO_XOUT_H_REG 0x43
#define PWR_MGMT_1_REG 0x6B
#define PWR_MGMT_2_REG 0x6C
#define WHO_AM_I_REG 0x75

#define M_PI 3.1415926f
#define MPU6050_MODE 2

//-----------------------------------------------	


 #if MPU6050_MODE == 0

/**
 * @if Eng
 * @brief Initializes MPU6050 in basic mode 
 * @param hi2c - Pointer to I2C handle structure 
 * @result MPU6050 is configured with default settings
 * @else
 * @brief 以基本模式初始化MPU6050 
 * @param hi2c - 指向I2C句柄结构的指针
 * @result MPU6050被配置为默认设置 
 * @endif 
 */
void MPU6050_Init(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Reads raw accelerometer data
 * @param ax - Pointer to store X-axis acceleration
 * @param ay - Pointer to store Y-axis acceleration
 * @param az - Pointer to store Z-axis acceleration
 * @result Raw accelerometer values stored in provided pointers
 * @else
 * @brief 读取原始加速度计数据
 * @param ax - 存储X轴加速度的指针 
 * @param ay - 存储Y轴加速度的指针
 * @param az - 存储Z轴加速度的指针 
 * @result 原始加速度值存储在提供的指针中 
 * @endif 
 */
void MPU6050_Read_Accel(int16_t *ax, int16_t *ay, int16_t *az);

/**
 * @if Eng
 * @brief Reads raw gyroscope data
 * @param gx - Pointer to store X-axis rotation 
 * @param gy - Pointer to store Y-axis rotation 
 * @param gz - Pointer to store Z-axis rotation
 * @result Raw gyroscope values stored in provided pointers 
 * @else 
 * @brief 读取原始陀螺仪数据
 * @param gx - 存储X轴旋转的指针
 * @param gy - 存储Y轴旋转的指针 
 * @param gz - 存储Z轴旋转的指针 
 * @result 原始陀螺仪值存储在提供的指针中 
 * @endif 
 */
void MPU6050_Read_Gyro(int16_t *gx, int16_t *gy, int16_t *gz);

/**
 * @if Eng
 * @brief Calibrates MPU6050 sensors
 * @param ax_offset - Pointer to store X-axis accelerometer offset 
 * @param ay_offset - Pointer to store Y-axis accelerometer offset
 * @param az_offset - Pointer to store Z-axis accelerometer offset 
 * @param gx_offset - Pointer to store X-axis gyroscope offset
 * @param gy_offset - Pointer to store Y-axis gyroscope offset
 * @param gz_offset - Pointer to store Z-axis gyroscope offset 
 * @result Calibration offsets calculated and stored 
 * @else 
 * @brief 校准MPU6050传感器
 * @param ax_offset - 存储X轴加速度计偏移的指针
 * @param ay_offset - 存储Y轴加速度计偏移的指针
 * @param az_offset - 存储Z轴加速度计偏移的指针
 * @param gx_offset - 存储X轴陀螺仪偏移的指针
 * @param gy_offset - 存储Y轴陀螺仪偏移的指针 
 * @param gz_offset - 存储Z轴陀螺仪偏移的指针 
 * @result 计算并存储校准偏移量
 * @endif 
 */
void MPU6050_Calibrate(int16_t *ax_offset, int16_t *ay_offset, int16_t *az_offset,int16_t *gx_offset, int16_t *gy_offset, int16_t *gz_offset);

 #elif MPU6050_MODE == 1

/**
 * @if Eng
 * @brief Initializes MPU6050 in raw data mode
 * @param hi2c - Pointer to I2C handle structure 
 * @result MPU6050 configured for raw data reading 
 * @else 
 * @brief 以原始数据模式初始化MPU6050 
 * @param hi2c - 指向I2C句柄结构的指针
 * @result MPU6050配置为原始数据读取 
 * @endif 
 */
void MPU6050_Init(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng
 * @brief Reads raw accelerometer and gyroscope data 
 * @param hi2c - Pointer to I2C handle structure
 * @param accel_data - Array to store [x,y,z] accelerometer data
 * @param gyro_data - Array to store [x,y,z] gyroscope data
 * @result Raw sensor data stored in provided arrays 
 * @else 
 * @brief 读取原始加速度计和陀螺仪数据 
 * @param hi2c - 指向I2C句柄结构的指针
 * @param accel_data - 存储[x,y,z]加速度计数据的数组 
 * @param gyro_data - 存储[x,y,z]陀螺仪数据的数组
 * @result 原始传感器数据存储在提供的数组中 
 * @endif 
 */
void MPU6050_Read_Raw(I2C_HandleTypeDef *hi2c,int16_t *accel_data, int16_t *gyro_data);

/**
 * @if Eng 
 * @brief Computes angles from sensor data
 * @param accel_data - Array containing accelerometer data 
 * @param gyro_data - Array containing gyroscope data 
 * @param dt - Time interval since last reading (seconds)
 * @result Angles computed using sensor fusion
 * @else
 * @brief 从传感器数据计算角度
 * @param accel_data - 包含加速度计数据的数组
 * @param gyro_data - 包含陀螺仪数据的数组 
 * @param dt - 自上次读数以来的时间间隔(秒)
 * @result 使用传感器融合计算角度 
 * @endif 
 */
void Compute_Angles(int16_t *accel_data, int16_t *gyro_data, float dt);

/**
 * @if Eng
 * @brief Sends sensor data via UART 
 * @param hi2c - Pointer to I2C handle structure
 * @param huart - Pointer to UART handle structure 
 * @param accel_data - Array containing accelerometer data 
 * @param gyro_data - Array containing gyroscope data 
 * @result Sensor data transmitted via UART 
 * @else 
 * @brief 通过UART发送传感器数据
 * @param hi2c - 指向I2C句柄结构的指针 
 * @param huart - 指向UART句柄结构的指针
 * @param accel_data - 包含加速度计数据的数组
 * @param gyro_data - 包含陀螺仪数据的数组 
 * @result 传感器数据通过UART传输
 * @endif 
 */
void MPU6050_SendData(I2C_HandleTypeDef *hi2c,UART_HandleTypeDef *huart,int16_t *accel_data, int16_t *gyro_data);

 #elif MPU6050_MODE == 2

/**
 * @if Eng
 * @brief Initializes MPU6050 in angle calculation mode
 * @param hi2c - Pointer to I2C handle structure 
 * @result MPU6050 configured for angle calculations 
 * @else 
 * @brief 以角度计算模式初始化MPU6050 
 * @param hi2c - 指向I2C句柄结构的指针
 * @result MPU6050配置为角度计算
 * @endif 
 */
void MPU6050_Init(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng
 * @brief Reads accelerometer data
 * @param hi2c - Pointer to I2C handle structure 
 * @result Accelerometer data read and stored internally 
 * @else 
 * @brief 读取加速度计数据
 * @param hi2c - 指向I2C句柄结构的指针 
 * @result 加速度计数据被读取并内部存储
 * @endif 
 */
void MPU6050_Read_Accel(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Reads gyroscope data
 * @param hi2c - Pointer to I2C handle structure 
 * @result Gyroscope data read and stored internally 
 * @else 
 * @brief 读取陀螺仪数据 
 * @param hi2c - 指向I2C句柄结构的指针
 * @result 陀螺仪数据被读取并内部存储
 * @endif 
 */
void MPU6050_Read_Gyro(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Computes and reads roll, pitch and yaw angles
 * @param hi2c - Pointer to I2C handle structure 
 * @result Euler angles calculated from sensor data 
 * @else 
 * @brief 计算并读取滚转、俯仰和偏航角
 * @param hi2c - 指向I2C句柄结构的指针 
 * @result 从传感器数据计算的欧拉角 
 * @endif 
 */
void MPU6050_Read_Result(I2C_HandleTypeDef *hi2c);//读roll, pitch, yaw

/**
 * @if Eng 
 * @brief Sends angle data via UART
 * @param hi2c - Pointer to I2C handle structure 
 * @param huart - Pointer to UART handle structure
 * @result Angle data transmitted via UART
 * @else
 * @brief 通过UART发送角度数据 
 * @param hi2c - 指向I2C句柄结构的指针
 * @param huart - 指向UART句柄结构的指针 
 * @result 角度数据通过UART传输
 * @endif 
 */
void MPU6050_SendData(I2C_HandleTypeDef *hi2c,UART_HandleTypeDef *huart);

 #elif MPU6050_MODE == 3

/**
 * @if Eng 
 * @brief Writes to MPU6050 register 
 * @param hi2c - Pointer to I2C handle structure
 * @param RegAddress - Register address to write 
 * @param Data - Data to write to register
 * @result Register value updated 
 * @else 
 * @brief 写入MPU6050寄存器
 * @param hi2c - 指向I2C句柄结构的指针 
 * @param RegAddress - 要写入的寄存器地址
 * @param Data - 要写入寄存器的数据
 * @result 寄存器值更新
 * @endif 
 */
void MPU6050_WriteReg(I2C_HandleTypeDef *hi2c,uint8_t RegAddress, uint8_t Data);

/**
 * @if Eng
 * @brief Reads from MPU6050 register 
 * @param hi2c - Pointer to I2C handle structure
 * @param RegAddress - Register address to read 
 * @return Value read from register
 * @else
 * @brief 从MPU6050寄存器读取 
 * @param hi2c - 指向I2C句柄结构的指针
 * @param RegAddress - 要读取的寄存器地址 
 * @return 从寄存器读取的值
 * @endif 
 */
uint8_t MPU6050_ReadReg(I2C_HandleTypeDef *hi2c,uint8_t RegAddress);

/**
 * @if Eng 
 * @brief Initializes MPU6050 in advanced mode
 * @param hi2c - Pointer to I2C handle structure 
 * @result MPU6050 configured with advanced settings 
 * @else 
 * @brief 以高级模式初始化MPU6050
 * @param hi2c - 指向I2C句柄结构的指针 
 * @result MPU6050配置为高级设置 
 * @endif 
 */
void MPU6050_Init(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Reads MPU6050 device ID 
 * @param hi2c - Pointer to I2C handle structure
 * @return Device ID value 
 * @else 
 * @brief 读取MPU6050设备ID 
 * @param hi2c - 指向I2C句柄结构的指针
 * @return 设备ID值
 * @endif 
 */
uint8_t MPU6050_GetID(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Reads accelerometer data 
 * @param hi2c - Pointer to I2C handle structure
 * @result Accelerometer data read and stored internally
 * @else
 * @brief 读取加速度计数据 
 * @param hi2c - 指向I2C句柄结构的指针
 * @result 加速度计数据被读取并内部存储 
 * @endif 
 */
void MPU6050_GetData_ACC(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng
 * @brief Reads gyroscope data 
 * @param hi2c - Pointer to I2C handle structure
 * @result Gyroscope data read and stored internally
 * @else
 * @brief 读取陀螺仪数据
 * @param hi2c - 指向I2C句柄结构的指针 
 * @result 陀螺仪数据被读取并内部存储 
 * @endif 
 */
void MPU6050_GetData_GYRO(I2C_HandleTypeDef *hi2c);

/**
 * @if Eng 
 * @brief Converts raw gyroscope data to physical values 
 * @param hi2c - Pointer to I2C handle structure
 * @param T_GyroX - Pointer to store X-axis gyro value
 * @param T_GyroY - Pointer to store Y-axis gyro value
 * @param T_GyroZ - Pointer to store Z-axis gyro value
 * @result Gyroscope data converted to physical units
 * @else
 * @brief 将原始陀螺仪数据转换为物理值
 * @param hi2c - 指向I2C句柄结构的指针 
 * @param T_GyroX - 存储X轴陀螺值的指针 
 * @param T_GyroY - 存储Y轴陀螺值的指针 
 * @param T_GyroZ - 存储Z轴陀螺值的指针 
 * @result 陀螺仪数据转换为物理单位 
 * @endif 
 */
void MPU6050_transition_GYRO(I2C_HandleTypeDef *hi2c,int16_t *T_GyroX, int16_t *T_GyroY, int16_t *T_GyroZ);

/**
 * @if Eng
 * @brief Converts raw accelerometer data to physical values 
 * @param hi2c - Pointer to I2C handle structure
 * @param T_AccX - Pointer to store X-axis accel value 
 * @param T_AccY - Pointer to store Y-axis accel value
 * @param T_AccZ - Pointer to store Z-axis accel value 
 * @result Accelerometer data converted to physical units
 * @else
 * @brief 将原始加速度计数据转换为物理值 
 * @param hi2c - 指向I2C句柄结构的指针
 * @param T_AccX - 存储X轴加速度值的指针
 * @param T_AccY - 存储Y轴加速度值的指针
 * @param T_AccZ - 存储Z轴加速度值的指针
 * @result 加速度计数据转换为物理单位 
 * @endif 
 */
void MPU6050_transition_ACC(I2C_HandleTypeDef *hi2c,int16_t *T_AccX, int16_t *T_AccY, int16_t *T_AccZ);

/**
 * @if Eng
 * @brief Updates AHRS (Attitude and Heading Reference System)
 * @param gx - X-axis gyroscope value 
 * @param gy - Y-axis gyroscope value
 * @param gz - Z-axis gyroscope value 
 * @param ax - X-axis accelerometer value
 * @param ay - Y-axis accelerometer value 
 * @param az - Z-axis accelerometer value
 * @result AHRS state updated with new sensor data 
 * @else 
 * @brief 更新AHRS(姿态航向参考系统)
 * @param gx - X轴陀螺仪值 
 * @param gy - Y轴陀螺仪值 
 * @param gz - Z轴陀螺仪值
 * @param ax - X轴加速度计值 
 * @param ay - Y轴加速度计值
 * @param az - Z轴加速度计值 
 * @result AHRS状态随新传感器数据更新
 * @endif 
 */
void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az);

/**
 * @if Eng
 * @brief Computes Euler angles from AHRS 
 * @param hi2c - Pointer to I2C handle structure 
 * @result Euler angles (roll, pitch, yaw) calculated
 * @else
 * @brief 从AHRS计算欧拉角
 * @param hi2c - 指向I2C句柄结构的指针 
 * @result 计算欧拉角(滚转、俯仰、偏航)
 * @endif 
 */
void ICM_getEulerianAngles(I2C_HandleTypeDef *hi2c);


 #endif




#endif

